/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef MAGNETICTRACK_H
#define MAGNETICTRACK_H
#include <QString>
#include <QVector>
#include "vehicledefine.h"
#include "gSoapFoundation/soapStub.h"
#include "MacroDefine/GeneralDefine.h"
#include "pid_controller.h"


//#define D_RFID_STOP_ID 1

class AxisObject;
//class VehicleKinematics;
class MotionMessage;
//struct PIDControl;提示冲突
/**
 * @brief The MagneticTrack class 磁条跟踪类：磁条导航和激光导航共用该类，将速度偏差进行pid运算后，输出轴速度和位置。
 */
class MagneticTrack
{
public:
    MagneticTrack(QString fileName, MotionMessage *motionMessageIn);

public:
    int getFrontMaganetic_deviceAlias();
    int getBackMaganetic_deviceAlias();
    int getFrontRfid_deviceAlias();
    int getBackRfid_deviceAlias();
    int changeToVmarkMode();
    int exitVmarkMode();
    int setMaganeticMovingSpeedRatio(double speedRatioIn);
    int getSafeSpeedRatio(double &ratioOut);
    int setVehicleDriveDirection(bool isPositiveDirectionIn);//这个以参数仅对磁条导航有效
    int getTrackStatus(E_TRACK_STATUS &statusOut);
    int setTrackMode(E_TRACK_MODE modeIn);
    int setStopCondition(E_TRACK_STOP_CONDITION conditionIn);
    int setMageneticPassRfid(long long passRfidIn);

    /**
     * @brief setVehicleIdealSpeed_ratio
     * @param speedIn :unit is %
     * @return
     */
    int setVehicleIdealSpeed_ratio(double speedIn);
    /**
     * @brief setVehicleIdealSpeed_mPerS
     * @param speedIn : unit is m/s
     * @return
     */
    int setVehicleIdealSpeed_mPerS(double speedIn);// m/s
    double getVehicleIdealSpeed_mPerS();// m/s
    /**
     * @brief setVehicleIdealRotation_ratio
     * @param speedIn : unit is %
     * @return
     */
    int setVehicleIdealRotation_ratio(double speedIn);// %
    int setStart();
    int setLaserSafeRatio(double ratioIn);
    int quickStop();
    int stop();
    int cease();
    int resetAll();
    int startRun();
    bool isInitialOk();
    /**
     * @brief trackLoop 磁条导航和激光导航共用该类，将速度偏差进行pid运算后，输出轴速度和位置。
     * @param sensorInfo
     * @param wheelVelOut unit is deg/s
     * @return
     */
    E_TRACK_LOOP_RESULT trackLoop(SensorInfo sensorInfo, QVector<double>& wheelVelOut, QVector<double> &steeringAngleOut,
                                  double &pidOut_xMove, double &pidOut_yMove
                                  , double &pidOut_rotateMove);
    /**
     * @brief getVirtualLineTrack_feedfowardOutput 获取前馈独立输出量
     * @param pidOut_xMove
     * @param pidOut_yMove
     * @param pidOut_rotateMove
     * @return
     */
    int getVirtualLineTrack_feedfowardOutput(double &pidOut_xMove, double &pidOut_yMove
                  , double &pidOut_rotateMove);
    int getErrorCode();
//    int resetError();
    int setMaxErrorCount(int maxCount);
    int setVehicleMagneticMovePid(int idOfPid, TrackPidInfo pidInfo);


    int getPidOutput(double &anglePidOutput, double &xPidOutput, double &yPidOutput);


private:
    int checkSensorDevice(const SensorInfo &sensorInfo);
//    int calculateVirtualLineIdealSpeed(double deltaDistanceIn, double &idealSpeedOut);
    int ultrasonicSensorProtectLoop(bool driveDirection, double &safeSpeedRatioOut);
    int readConfigFile(QString fileName);
    int initialPidController_maganetic();
    int initialPidController_virtualLine_rotate();
    int initialPidController_virtualLine_xMove();
    int initialPidController_virtualLine_yMove();
    int pidOutput2WheelVel_virtualLine(double pidOut_x,double pidOut_r, QVector<double> &wheelVelOut);//unit is deg/s
    int pidOutput2WheelVel_twoDifferentialDriver_virtualLine(double pidOut_x,double pidOut_r,
                                                             QVector<double> &wheelVelOut);

    int pidOutput2WheelVel_maganeticTrack(QVector<double> &steeringAngleIn, double pidDriveValue,
                          QVector<double> &wheelVelOut, QVector<double> &steeringAngleOut);//unit is deg/s
    int pidOutput2WheelVel_twoDifferentialDriver_line_maganetic(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_twoDifferentialDriver_angle_maganetic(double pidDriveValue, QVector<double> &wheelVelOut);//not used
    int pidOutput2WheelVel_oneSterringDriver_line_maganetic(QVector<double> &steeringAngleIn,
                               double pidDriveValue, QVector<double> &wheelVelOut, double &steeringAngleOut);
    int pidOutput2WheelVel_oneSterringDriver_angle_maganetic(double pidDriveValue, QVector<double> &wheelVelOut);//not used
    int pidOutput2WheelVel_twoSterringDriver_line_maganetic(double pidDriveValue, QVector<double> &wheelVelOut);
    int pidOutput2WheelVel_twoSterringDriver_angle_maganetic(double pidDriveValue, QVector<double> &wheelVelOut);//not used

    int pidCalculateOut(E_TRACK_MODE trackModeIn, QVector<double> &steeringAngleIn,
                        double x_error, double y_error, double r_error,
                        double x_feedforward, double y_feedforward, double r_feedforward,
                        QVector<double>& wheelVelOut, QVector<double> &steeringAngleOut, double &pidOut_xMove, double &pidOut_yMove, double &pidOut_rotateMove);
    int pidInputCalculate(E_TRACK_MODE trackModeIn, bool isPositiveDirectionIn, SensorInfo sensorInfoIn,
                          double &resultOut_x,double &resultOut_y, double &resultOut_r);
    int pidInputCalculate_maganetic_middle(QVector<bool> magneticData, double &resultOut);
    int pidInputCalculate_maganetic_low(QVector<bool> magneticData, double &resultOut);
    int pidInputCalculate_maganetic_high(QVector<bool> magneticData, double &resultOut);
//    int pidInputCalculate_virtualLine(bool isPositiveDirectionIn, double absoluteAngle, double deltaAngle,
//                                      double absoluteDistance, double &resultOut);
    E_STOP_STATUS passConditionLoop(SensorInfo sensorInfoIn);
    E_STOP_STATUS stopConditionLoop(SensorInfo sensorInfoIn,double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_input(bool inputDi, double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_rfid(bool isRfidDetected, long long rfidData, double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_maganeticWidth(QVector<bool> magneticData,double &slowdownRatioOut);
    E_STOP_STATUS stopCondition_virtualLine(double deltaDistance,double deltaAngle,double &slowdownRatioOut);//not used

    int initialSpeedRatioAxis();
    double getSpeedRatioAxisScale();
    int setSpeedRatioAxisScale(double ratio, double timeDuration);
    int speedRatioAxisMoveOneStep();
    int resetSpeedRatioAxis();
    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, double parameter1=0, double parameter2=0,
                int parameter3=0, int parameter4=0);

private:
//    VehicleKinematics* vehicleKinematic;
    E_VEHICLE_TYPE vehicletype;
    double trackDirectionAngle;//双舵轮时，追踪路线与双舵轮连线的夹角。涉及到不同控制策略。
    bool initialFlag;
    double lastSensorInfo;
    double sensorTotalWidth;
    double magneticPidParameter_P;
    double magneticPidParameter_I;
    double magneticPidParameter_D;
    double magneticPidParameter_feedforward;
    double magneticPidParameter_errorLimitForIntegration;

    double lineSpeedAdjustMax_maganetic;//m/s,//单轮偏差调整最大值
    double angleSpeedAdjustMax_maganetic;//degree/s 旋转运动最大偏差补偿
    double vehicleVelocityConpensation_maganetic;//速度比例补偿

    double sampleTime;
    int frontMaganeticDirection;
    int backMaganeticDirection;


    PIDControl *pidController_maganetic;//磁条导航用.计算r方向速度控制
    PIDControl *pidController_rotateMove;//激光导航用于计算r方向速度控制
    PIDControl *pidController_xMove;//激光导航用于计算x方向速度控制
    PIDControl *pidController_yMove;//激光导航用于计算y方向速度控制,全向轮用。

    int errorCount;
    int maxErrorCount;

    E_TRACK_STATUS status;
    double idealSpeed;// unit m/s
    double composedLineSpeed;//与减速速率合成后的速度
    double idealRotationSpeed;//unit is rad/s
    double composedRotationSpeed;
    double slowdownSpeedRatio;//减速速率
    double safeSpeedRatio;//障碍物安全减速速率
    double ultrasonicSpeedRatio;//超声波安全减速速率
    double laserSafeRatio;//激光雷达探测的安全系数

    VehicleVelocity vehicleMaxVel;
    double wheelDiameter;
    double wheelDistance;
    double wheelDistance_height;
    double wheelMaxSpeed;

    bool isPositiveDirection;
    bool isLineMove;
    E_TRACK_MODE trackMode;
    E_TRACK_STOP_CONDITION stopCondition;

    double  allowedReachOffset_line;//M
    double  allowedReachOffset_angle;//degree

    AxisObject* speedRatioAxis;//速度比例轴
    int stopStage;
    MotionMessage *motionMessage;
    int maganeticWidthCount_stop;//停止磁感应灯个数
    double slowdownRange_line;
    double slowdownRange_angle;

    int rfidSensorCount;
    int maganeticSensorCount;
    long long passRfid;

    double moveX_PidParameter_P;
    double moveX_PidParameter_P_vmark;
    double moveX_PidParameter_I;
    double moveX_PidParameter_D;
    double moveX_PidParameter_feedforward;
    double moveX_PidParameter_errorLimitForIntegration;

    double moveY_PidParameter_P;
    double moveY_PidParameter_P_vmark;
    double moveY_PidParameter_I;
    double moveY_PidParameter_D;
    double moveY_PidParameter_feedforward;
    double moveY_PidParameter_errorLimitForIntegration;

    double moveR_PidParameter_P;
    double moveR_PidParameter_I;
    double moveR_PidParameter_D;
    double moveR_PidParameter_feedforward;
    double moveR_PidParameter_errorLimitForIntegration;

    double virtualLinePathLineSpeedAdjustMax;//直线偏差调整最大值
    double virtualLineAngleSpeedAdjustMax;//旋转运动最大偏差补偿
    double virtualLinePathVehicleVelocityConpensation;//速度比例补偿

    double userSpeedRatio;//用户速度比例系数。not used
    int frontMaganetic_deviceAlias;
    int backMaganetic_deviceAlias;
    int frontRfid_deviceAlias;
    int backRfid_deviceAlias;
    bool isUseFrontMaganetic;
    bool isUseBackMaganetic;
    bool isUseFrontRfid;
    bool isUseBackRfid;
    int robotId;

};

#endif // MAGNETICTRACK_H
